clear;
exec('circshift.sci');
exec('std.sci');
N = 4096;
Na = 3;
main = [7,8];
per = [1,2,3,4,5,6];//,5,6,7,8];
dim = 2;
n_delay = 2;

Xadc = csvRead('Xadc20.csv');
E = Xadc(main,1:N);
Z = Xadc(per,1:N);
//Z(main,:) = [];

a = zeros(dim*Na*(n_delay + 1), dim);
b = zeros(dim*dim*Na*n_delay, dim);
Za = zeros(2*Na*(n_delay+1),N);
k_fir = 0.1;
k_iir = 0.001;
for i=0:n_delay
    Za(2*Na*i+1:2*Na*(i+1),:) = circshift(Z(:,1:N)',i)';
end
for iter = 1:10
    ///////////////////////// FIR /////////////////////////////
    Rzza = Za*Za';
    Rzea = Za*E';
    invRzza = inv(Rzza);
    a = a + k_fir*invRzza * Rzea;
    //Z_ = a'*Za;

    Za_ = zeros(dim*dim*Na,N);
    for i=1:dim*Na
        Za_((i-1)*dim+1:(i-1)*dim+dim,:) = a(i:dim*Na:$,:)' * Za(i:dim*Na:$,:);
    end
    /////////////////////////////////////////////////////////
    ////////////////////////// IIR //////////////////////////
    Zb = zeros(dim*dim*Na*n_delay,N);
//    for i=0:n_delay-1
//        Zb(i*dim*dim*Na+1:(i+1)*dim*dim*Na,:) = circshift(Za_(:,1:N)',i+1)';
//    end
//
//    Rzzb = Zb*Zb';
//    Rzeb = Zb*(E - a'*Za)';
//    invRzzb = inv(Rzzb);
//    b = b + k_iir*invRzzb * Rzeb;
//    //Z_ = b'*Zb;
//
//    Zb_ = zeros(dim*dim*Na,N);
//    for i=1:dim*Na
//        Zb_((i-1)*dim+1:(i-1)*dim+dim,:) = b(i:dim*Na:$,:)' * Zb(i:dim*Na:$,:);
//    end
end
///////////////////////////////////////////////////////
////////////////// OUT ////////////////////////////////
w = [a;b];
Z_ = [Za; Zb];
Zout = w'*Z_;
//Zout = Z_;

db = 20*log10(std(E(1,:))/std(E(1,:)-Zout(1,:)));
disp(db);
